#include "mpc.h"
#include <iostream>
#include <memory>
#include "common_tool/csv_writer.h"
#include "eigen_csv.h"
using namespace LinearMpc;
int main()
{
	////////////////////////////////////////////////////////////////////////////////////////////////////////
	{ //(uint state_dim,uint control_dim, uint distur_dim)
		//Mpc::Mpc(std::shared_ptr<DiscreteLinearSys> dis_sys_ptr, uint mpc_windows, uint max_xu_constraint, uint max_final_constraint)
		std::string prefix = "case1";

		int state_dim = 4;
		int control_dim = 3;
		int distur_dim = 2;
		int mpc_windows = 3;
		int max_xu_constraint = 2;
		int max_final_constraint = 2;
		std::shared_ptr<DiscreteLinearSys> dis_sys_ptr(new DiscreteLinearSys(state_dim, control_dim, distur_dim));

		Eigen::MatrixXd state_matrix(state_dim, state_dim);
		state_matrix << 1.1, 1.2, 1.3, 1.4,
			1.5, 1.6, 1.7, 1.8,
			2.1, 2.2, 2.3, 2.4,
			2.5, 2.6, 2.7, 2.8;

		dis_sys_ptr->set_state_matrix(state_matrix);
		std::cout << "state matrix is:" << std::endl;
		std::cout << state_matrix << std::endl;
		Eigen::MatrixXd control_matrix(state_dim, control_dim);
		control_matrix << 3.1, 3.2, 3.3,
			3.5, 3.6, 3.7,
			4.1, 4.1, 4.3,
			4.5, 4.6, 4.7;
		std::cout << "control matrix is:" << std::endl;
		std::cout << control_matrix << std::endl;
		dis_sys_ptr->set_control_matrix(control_matrix);

		Eigen::MatrixXd distur_matrix(state_dim, distur_dim);
		distur_matrix << 0.1, 0.2,
			0.2, 0.3,
			0.4, 0.5,
			0.6, 0.7;
		dis_sys_ptr->set_distur_matrix(distur_matrix);
		std::cout << "distur_matrix is:" << std::endl;
		std::cout << distur_matrix << std::endl;
		//Mpc(std::shared_ptr<DiscreteLinearSys> dis_sys_ptr, unsigned int mpc_windows,
		//unsigned int max_xu_constraint, unsigned int max_delta_x, unsigned int max_delta_u, unsigned int max_final_constraint);
		Mpc test_mpc(dis_sys_ptr, mpc_windows, max_xu_constraint, 1, 1, max_final_constraint);

		//test defualt hessian
		Eigen::MatrixXd temp_matrix = Eigen::MatrixXd(test_mpc.hessian());

		std::cout << prefix + " defualt hessian set is :" << std::endl;
		std::cout << temp_matrix << std::endl;
		eigen_to_csv(prefix + "_defualt_hessian.csv", temp_matrix);

		//test defualt gradient
		std::cout << prefix + " defualt gradient is:" << std::endl;
		std::cout << test_mpc.gradient().transpose() << std::endl;
		eigen_to_csv(prefix + "_defualt_gradient.csv", test_mpc.gradient());
		//test defualt constraint matrix

		temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
		std::cout << prefix + " defualt constriant_matrix is ok:" << std::endl;
		eigen_to_csv(prefix + "_defualt_constriant_matrix.csv", test_mpc.constraint_matrix());
		std::cout << temp_matrix << std::endl;
		std::cout << prefix + " default bound is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_defualt_lowerBound.csv", test_mpc.lowerBound());
		std::cout << test_mpc.upperBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_defualt_upperBound.csv", test_mpc.upperBound());
		//error msg will be apear
		test_mpc.set_xu_affine({0, 2}, {1.0}, {}, {}, 0, 0, 0);
		/*int Mpc::set_xu_affine(const std::vector<double> x_index, const std::vector<double> x_factor,
	const std::vector<double> u_index, const std::vector<double> u_factor,
	int constraint_index, double min, double max)*/
		test_mpc.set_xu_affine({0, 2}, {1.0, 2.0}, {1, 2}, {1.0, 1.0}, 0, -1, 1);
		test_mpc.set_xu_affine({1, 3}, {1.1, 2.1}, {0, 1}, {1.2, 1.2}, 1, -2, 2);
		//int set_delta_u(unsigned int constraint_index, unsigned int u_index, double min, double max);
		test_mpc.set_delta_u(0, 1, -1000.0, 1000.0);
		test_mpc.set_delta_x(0, 0, -1000.0, 1000.0);
		//int set_final_constraint(int constraint_index, const std::vector<double> x_index, const std::vector<double> x_factor,
		//						 double min, double max);
		test_mpc.set_final_constraint(0, {0, 1, 2, 3}, {0.1, 0.2, 0.3, 0.4}, -0.5, 0.5);
		test_mpc.set_final_constraint(1, {0, 1, 2, 3}, {0.1, 0.3, 0.5, 0.7}, -1.0, 1.0);
		temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
		std::cout << prefix + " constriant_matrix affter add  xu_affine 0 is:" << std::endl;
		std::cout << temp_matrix << std::endl;
		eigen_to_csv(prefix + "_constriant_matrix_add_affine0.csv", temp_matrix);
		std::cout << prefix + " bound affter add  xu_affine 0 is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_lowerBound_add_affine0.csv", test_mpc.lowerBound());
		std::cout << test_mpc.upperBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_upperBound_add_affine0.csv", test_mpc.upperBound());
		/*	bool update_mpc(const Eigen::VectorXd &x0, const std::vector<double> &u_fb,
					const Eigen::MatrixXd &x_ref, const Eigen::MatrixXd &u_ref, const Eigen::MatrixXd &distur_ref,
					const Eigen::MatrixXd &q, const Eigen::MatrixXd &r, const std::vector<double> &xu_ae_w = {},
					const std::vector<double> &xu_slack_ineq_w = {}, const std::vector<double> &x_final_ae_w = {},
					const std::vector<double> &x_final_slack_ineq_w = {}, const std::vector<double> &delta_u_ae_w = {},
					const std::vector<double> &delta_u_slack_ineq_w = {}, const std::vector<double> &delta_x_ae_w = {},
					const std::vector<double> &delta_x_slack_ineq_w = {});*/
		//set u-1
		Eigen::VectorXd x0(state_dim);
		x0 << 1.1, 1.2, 1.3, 1.4;
		Eigen::VectorXd distur_ref(distur_dim);
		distur_ref << 1.0, 2.0;
		Eigen::MatrixXd q(state_dim, state_dim);
		q << 1.1, 0, 0, 0,
			0, 1.2, 0, 0,
			0, 0, 1.3, 0,
			0, 0, 0, 1.4;
		Eigen::MatrixXd r(control_dim, control_dim);
		r << 1.8, 0, 0,
			0, 1.9, 0,
			0, 0, 1.5;
		Eigen::VectorXd x_ref(state_dim);
		x_ref << 1.0, 2.0, 3.0, 4.0;
		Eigen::VectorXd u_ref(control_dim);
		u_ref << 1.0, 2.0, 3.0;
		Eigen::VectorXd u_fb(3);
		u_fb << 0.1, 0.2, 0.3;
		test_mpc.update_mpc(x0, u_fb, x_ref, u_ref, distur_ref, q, r);
		temp_matrix = Eigen::MatrixXd(test_mpc.hessian());
		std::cout << prefix + " hessian update1 is :" << std::endl;
		std::cout << temp_matrix << std::endl;
		eigen_to_csv(prefix + "_hessian_update1.csv", temp_matrix);
		//test defualt gradient
		std::cout << prefix + " gradient_update1 is:" << std::endl;
		std::cout << test_mpc.gradient().transpose() << std::endl;
		eigen_to_csv(prefix + "_gradient_update1.csv", test_mpc.gradient());
		temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
		std::cout << prefix + " constriant_matrix update1 is ok:" << std::endl;
		eigen_to_csv(prefix + "_constriant_matrix_update1.csv", test_mpc.constraint_matrix());
		std::cout << temp_matrix << std::endl;
		std::cout << prefix + " update1 bound is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_lowerBound_update1.csv", test_mpc.lowerBound());
		std::cout << test_mpc.upperBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_upperBound_update1.csv", test_mpc.upperBound());

		temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
		std::cout << prefix + " constriant_matrix affter update1  is:" << std::endl;
		std::cout << temp_matrix << std::endl;
		eigen_to_csv(prefix + "_constriant_matrix_update1.csv", temp_matrix);
		std::cout << prefix + " bound affter affter update1 is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_lowerBound_update1.csv", test_mpc.lowerBound());
		std::cout << test_mpc.upperBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_upperBound_update1.csv", test_mpc.upperBound());
		test_mpc.mpc_solve();
	}
	///////////////////////////////////////////////////////////////////////////////////////////////
	{
		//add xu affine constriant
		std::string prefix = "case2";
		int state_dim = 4;
		int control_dim = 3;
		int distur_dim = 2;
		int mpc_windows = 3;
		int max_xu_constraint = 2;
		int max_final_constraint = 2;
		std::shared_ptr<DiscreteLinearSys> dis_sys_ptr(new DiscreteLinearSys(state_dim, control_dim, distur_dim));

		Eigen::MatrixXd state_matrix(state_dim, state_dim);
		state_matrix << 1.1, 1.2, 1.3, 1.4,
			1.5, 1.6, 1.7, 1.8,
			2.1, 2.2, 2.3, 2.4,
			2.5, 2.6, 2.7, 2.8;

		dis_sys_ptr->set_state_matrix(state_matrix);
		std::cout << "state matrix is:" << std::endl;
		std::cout << state_matrix << std::endl;
		Eigen::MatrixXd control_matrix(state_dim, control_dim);
		control_matrix << 3.1, 3.2, 3.3,
			3.5, 3.6, 3.7,
			4.1, 4.1, 4.3,
			4.5, 4.6, 4.7;
		std::cout << "control matrix is:" << std::endl;
		std::cout << control_matrix << std::endl;
		dis_sys_ptr->set_control_matrix(control_matrix);

		Eigen::MatrixXd distur_matrix(state_dim, distur_dim);
		distur_matrix << 0.1, 0.2,
			0.2, 0.3,
			0.4, 0.5,
			0.6, 0.7;
		dis_sys_ptr->set_distur_matrix(distur_matrix);
		std::cout << "distur_matrix is:" << std::endl;
		std::cout << distur_matrix << std::endl;
		//Mpc(std::shared_ptr<DiscreteLinearSys> dis_sys_ptr, unsigned int mpc_windows,
		/*Mpc(std::shared_ptr<DiscreteLinearSys> dis_sys_ptr,
		unsigned int mpc_windows,
		unsigned int max_xu_constraint,
		unsigned int max_delta_x,
		unsigned int max_delta_u,
		unsigned int max_final_constraint,
		unsigned int max_xu_ae = 0,
		unsigned int max_xu_slack_ineq = 0,
		unsigned int max_x_final_ae = 0,
		unsigned int max_x_final_slack_ineq = 0,
		unsigned int max_delta_u_ae = 0,
		unsigned int max_delta_u_slack_ineq = 0,
		unsigned int max_delta_x_ae = 0,
		unsigned int max_delta_x_slack_ineq = 0);*/
		Mpc test_mpc(dis_sys_ptr, mpc_windows, max_xu_constraint, 1, 1, max_final_constraint, 1);

		//test defualt hessian
		Eigen::MatrixXd temp_matrix = Eigen::MatrixXd(test_mpc.hessian());

		std::cout << prefix + " defualt hessian set is :" << std::endl;
		std::cout << temp_matrix << std::endl;
		eigen_to_csv(prefix + "_defualt_hessian.csv", temp_matrix);

		//test defualt gradient
		std::cout
			<< "prefix defualt gradient is:" << std::endl;
		std::cout << test_mpc.gradient().transpose() << std::endl;
		eigen_to_csv(prefix + "_defualt_gradient.csv", test_mpc.gradient());
		//test defualt constraint matrix

		temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
		std::cout << prefix + " defualt constriant_matrix is ok:" << std::endl;
		eigen_to_csv(prefix + "_defualt_constriant_matrix.csv", test_mpc.constraint_matrix());
		std::cout << temp_matrix << std::endl;
		std::cout << prefix + " default bound is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_defualt_lowerBound.csv", test_mpc.lowerBound());
		std::cout << test_mpc.upperBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_defualt_upperBound.csv", test_mpc.upperBound());
		test_mpc.set_xu_affine({0, 2}, {1.0}, {}, {}, 0, 0, 0);
		/*int Mpc::set_xu_affine(const std::vector<double> x_index, const std::vector<double> x_factor,
	const std::vector<double> u_index, const std::vector<double> u_factor,
	int constraint_index, double min, double max)*/
		test_mpc.set_xu_affine({0, 2}, {1.0, 2.0}, {1, 2}, {1.0, 1.0}, 0, -1, 1);
		test_mpc.set_xu_affine({1, 3}, {1.1, 2.1}, {0, 1}, {1.2, 1.2}, 1, -2, 2);
		//int set_delta_u(unsigned int constraint_index, unsigned int u_index, double min, double max);
		test_mpc.set_delta_u(0, 1, -1000.0, 1000.0);
		test_mpc.set_delta_x(0, 0, -1000.0, 1000.0);
		//int set_final_constraint(int constraint_index, const std::vector<double> x_index, const std::vector<double> x_factor,
		//						 double min, double max);
		test_mpc.set_final_constraint(0, {0, 1, 2, 3}, {0.1, 0.2, 0.3, 0.4}, -0.5, 0.5);
		test_mpc.set_final_constraint(1, {0, 1, 2, 3}, {0.1, 0.3, 0.5, 0.7}, -1.0, 1.0);
		temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
		std::cout << prefix + " constriant_matrix affter add  xu_affine 0 is:" << std::endl;
		std::cout << temp_matrix << std::endl;
		eigen_to_csv(prefix + " constriant_matrix_add_affine0.csv", temp_matrix);
		std::cout << prefix + " bound affter add  xu_affine 0 is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_lowerBound_add_affine0.csv", test_mpc.lowerBound());
		std::cout << test_mpc.upperBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_upperBound_add_affine0.csv", test_mpc.upperBound());
		//set u-1
		Eigen::VectorXd x0(state_dim);
		x0 << 1.1, 1.2, 1.3, 1.4;
		Eigen::VectorXd distur_ref(distur_dim);
		distur_ref << 1.0, 2.0;
		Eigen::MatrixXd q(state_dim, state_dim);
		q << 1.1, 0, 0, 0,
			0, 1.2, 0, 0,
			0, 0, 1.3, 0,
			0, 0, 0, 1.4;
		Eigen::MatrixXd r(control_dim, control_dim);
		r << 1.8, 0, 0,
			0, 1.9, 0,
			0, 0, 1.5;
		Eigen::VectorXd x_ref(state_dim);
		x_ref << 1.0, 2.0, 3.0, 4.0;
		Eigen::VectorXd u_ref(control_dim);
		u_ref << 1.0, 2.0, 3.0;
		/*	bool update_mpc(const Eigen::VectorXd &x0, const std::vector<double> &u_fb,
			const Eigen::MatrixXd &x_ref, const Eigen::MatrixXd &u_ref, const Eigen::MatrixXd &distur_ref,
			const Eigen::MatrixXd &q, const Eigen::MatrixXd &r, const std::vector<double> &xu_ae_w = {},
			const std::vector<double> &xu_slack_ineq_w = {}, const std::vector<double> &x_final_ae_w = {},
			const std::vector<double> &x_final_slack_ineq_w = {}, const std::vector<double> &delta_u_ae_w = {},
			const std::vector<double> &delta_u_slack_ineq_w = {}, const std::vector<double> &delta_x_ae_w = {},
			const std::vector<double> &delta_x_slack_ineq_w = {});*/
		//	int set_xu_ae(const std::vector<double> x_index, const std::vector<double> x_factor,
		//const std::vector<double> u_index, const std::vector<double> u_factor,
		//int constraint_index, double offset = 0.0);

		test_mpc.set_xu_ae({1, 3}, {0.1, 0.2}, {1, 2}, {0.5, 0.6}, 0, 0.1);
		test_mpc.set_xu_ae_offset(0, 0.2);
		Eigen::VectorXd u_fb(3);
		u_fb << 0.1, 0.2, 0.3;
		test_mpc.update_mpc(x0, u_fb, x_ref, u_ref, distur_ref, q, r, {2.3});
		temp_matrix = Eigen::MatrixXd(test_mpc.hessian());
		std::cout << prefix + " hessian update1 is :" << std::endl;
		std::cout << temp_matrix << std::endl;
		eigen_to_csv(prefix + "_hessian_update1.csv", temp_matrix);
		//test defualt gradient
		std::cout << prefix + " gradient_update1 is:" << std::endl;
		std::cout << test_mpc.gradient().transpose() << std::endl;
		eigen_to_csv(prefix + "_gradient_update1.csv", test_mpc.gradient());
		temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
		std::cout << prefix + " constriant_matrix update1 is ok:" << std::endl;
		eigen_to_csv(prefix + "_constriant_matrix_update1.csv", test_mpc.constraint_matrix());
		std::cout << temp_matrix << std::endl;
		std::cout << prefix + " update1 bound is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_lowerBound_update1.csv", test_mpc.lowerBound());
		std::cout << test_mpc.upperBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_upperBound_update1.csv", test_mpc.upperBound());

		temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
		std::cout << prefix + " constriant_matrix affter update1  is:" << std::endl;
		std::cout << temp_matrix << std::endl;
		eigen_to_csv(prefix + "_constriant_matrix_update1.csv", temp_matrix);
		std::cout << prefix + " bound affter affter update1 is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_lowerBound_update1.csv", test_mpc.lowerBound());
		std::cout << test_mpc.upperBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_upperBound_update1.csv", test_mpc.upperBound());
		test_mpc.mpc_solve();
	}
	///////////////////////////////////////////////////////////////////////////////////////////////
	{
		//add xu_ineq constriant
		std::string prefix = "case3";
		int state_dim = 4;
		int control_dim = 3;
		int distur_dim = 2;
		int mpc_windows = 3;
		int max_xu_constraint = 2;
		int max_final_constraint = 2;
		std::shared_ptr<DiscreteLinearSys> dis_sys_ptr(new DiscreteLinearSys(state_dim, control_dim, distur_dim));

		Eigen::MatrixXd state_matrix(state_dim, state_dim);
		state_matrix << 1.1, 1.2, 1.3, 1.4,
			1.5, 1.6, 1.7, 1.8,
			2.1, 2.2, 2.3, 2.4,
			2.5, 2.6, 2.7, 2.8;

		dis_sys_ptr->set_state_matrix(state_matrix);
		std::cout << "state matrix is:" << std::endl;
		std::cout << state_matrix << std::endl;
		Eigen::MatrixXd control_matrix(state_dim, control_dim);
		control_matrix << 3.1, 3.2, 3.3,
			3.5, 3.6, 3.7,
			4.1, 4.1, 4.3,
			4.5, 4.6, 4.7;
		std::cout << "control matrix is:" << std::endl;
		std::cout << control_matrix << std::endl;
		dis_sys_ptr->set_control_matrix(control_matrix);

		Eigen::MatrixXd distur_matrix(state_dim, distur_dim);
		distur_matrix << 0.1, 0.2,
			0.2, 0.3,
			0.4, 0.5,
			0.6, 0.7;
		dis_sys_ptr->set_distur_matrix(distur_matrix);
		std::cout << "distur_matrix is:" << std::endl;
		std::cout << distur_matrix << std::endl;
		//Mpc(std::shared_ptr<DiscreteLinearSys> dis_sys_ptr, unsigned int mpc_windows,
		/*Mpc(std::shared_ptr<DiscreteLinearSys> dis_sys_ptr,
		unsigned int mpc_windows,
		unsigned int max_xu_constraint,
		unsigned int max_delta_x,
		unsigned int max_delta_u,
		unsigned int max_final_constraint,
		unsigned int max_xu_ae = 0,
		unsigned int max_xu_slack_ineq = 0,
		unsigned int max_x_final_ae = 0,
		unsigned int max_x_final_slack_ineq = 0,
		unsigned int max_delta_u_ae = 0,
		unsigned int max_delta_u_slack_ineq = 0,
		unsigned int max_delta_x_ae = 0,
		unsigned int max_delta_x_slack_ineq = 0);*/
		Mpc test_mpc(dis_sys_ptr, mpc_windows, max_xu_constraint, 1, 1, max_final_constraint, 1, 1);

		//test defualt hessian
		Eigen::MatrixXd temp_matrix = Eigen::MatrixXd(test_mpc.hessian());

		std::cout << prefix + " defualt hessian set is :" << std::endl;
		std::cout << temp_matrix << std::endl;
		eigen_to_csv(prefix + "_defualt_hessian.csv", temp_matrix);

		//test defualt gradient
		std::cout
			<< "prefix defualt gradient is:" << std::endl;
		std::cout << test_mpc.gradient().transpose() << std::endl;
		eigen_to_csv(prefix + "_defualt_gradient.csv", test_mpc.gradient());
		//test defualt constraint matrix

		temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
		std::cout << prefix + " defualt constriant_matrix is ok:" << std::endl;
		eigen_to_csv(prefix + "_defualt_constriant_matrix.csv", test_mpc.constraint_matrix());
		std::cout << temp_matrix << std::endl;
		std::cout << prefix + " default bound is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_defualt_lowerBound.csv", test_mpc.lowerBound());
		std::cout << test_mpc.upperBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_defualt_upperBound.csv", test_mpc.upperBound());
		test_mpc.set_xu_affine({0, 2}, {1.0}, {}, {}, 0, 0, 0);
		/*int Mpc::set_xu_affine(const std::vector<double> x_index, const std::vector<double> x_factor,
	const std::vector<double> u_index, const std::vector<double> u_factor,
	int constraint_index, double min, double max)*/
		test_mpc.set_xu_affine({0, 2}, {1.0, 2.0}, {1, 2}, {1.0, 1.0}, 0, -1, 1);
		test_mpc.set_xu_affine({1, 3}, {1.1, 2.1}, {0, 1}, {1.2, 1.2}, 1, -2, 2);
		//int set_delta_u(unsigned int constraint_index, unsigned int u_index, double min, double max);
		test_mpc.set_delta_u(0, 1, -1000.0, 1000.0);
		test_mpc.set_delta_x(0, 0, -1000.0, 1000.0);
		//int set_final_constraint(int constraint_index, const std::vector<double> x_index, const std::vector<double> x_factor,
		//						 double min, double max);
		test_mpc.set_final_constraint(0, {0, 1, 2, 3}, {0.1, 0.2, 0.3, 0.4}, -0.5, 0.5);
		test_mpc.set_final_constraint(1, {0, 1, 2, 3}, {0.1, 0.3, 0.5, 0.7}, -1.0, 1.0);
		temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
		std::cout << prefix + " constriant_matrix affter add  xu_affine 0 is:" << std::endl;
		std::cout << temp_matrix << std::endl;
		eigen_to_csv(prefix + " constriant_matrix_add_affine0.csv", temp_matrix);
		std::cout << prefix + " bound affter add  xu_affine 0 is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_lowerBound_add_affine0.csv", test_mpc.lowerBound());
		std::cout << test_mpc.upperBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_upperBound_add_affine0.csv", test_mpc.upperBound());
		//set u-1
		Eigen::VectorXd x0(state_dim);
		x0 << 1.1, 1.2, 1.3, 1.4;
		Eigen::VectorXd distur_ref(distur_dim);
		distur_ref << 1.0, 2.0;
		Eigen::MatrixXd q(state_dim, state_dim);
		q << 1.1, 0, 0, 0,
			0, 1.2, 0, 0,
			0, 0, 1.3, 0,
			0, 0, 0, 1.4;
		Eigen::MatrixXd r(control_dim, control_dim);
		r << 1.8, 0, 0,
			0, 1.9, 0,
			0, 0, 1.5;
		Eigen::VectorXd x_ref(state_dim);
		x_ref << 1.0, 2.0, 3.0, 4.0;
		Eigen::VectorXd u_ref(control_dim);
		u_ref << 1.0, 2.0, 3.0;
		Eigen::VectorXd u_fb(3);
		u_fb << 0.1, 0.2, 0.3;
		//	int set_xu_ae(const std::vector<double> x_index, const std::vector<double> x_factor,
		//const std::vector<double> u_index, const std::vector<double> u_factor,
		//int constraint_index, double offset = 0.0);

		test_mpc.set_xu_ae({1, 3}, {0.1, 0.2}, {1, 2}, {0.5, 0.6}, 0, 0.1);
		test_mpc.set_xu_ae_offset(0, 0.2);
		/*int set_xu_slack_ineq(const std::vector<double> x_index, const std::vector<double> x_factor,
						  const std::vector<double> u_index, const std::vector<double> u_factor,
						  int constraint_index, double min, double max);*/
		test_mpc.set_xu_slack_ineq({1, 3}, {0.3, 0.4}, {1, 2}, {0.7, 0.8}, 0, -1000, 1000);
		test_mpc.set_xu_slack_bound(0, -2000, 2000);
		/*	bool update_mpc(const Eigen::VectorXd &x0, const std::vector<double> &u_fb,
			const Eigen::MatrixXd &x_ref, const Eigen::MatrixXd &u_ref, const Eigen::MatrixXd &distur_ref,
			const Eigen::MatrixXd &q, const Eigen::MatrixXd &r, const std::vector<double> &xu_ae_w = {},
			const std::vector<double> &xu_slack_ineq_w = {}, const std::vector<double> &x_final_ae_w = {},
			const std::vector<double> &x_final_slack_ineq_w = {}, const std::vector<double> &delta_u_ae_w = {},
			const std::vector<double> &delta_u_slack_ineq_w = {}, const std::vector<double> &delta_x_ae_w = {},
			const std::vector<double> &delta_x_slack_ineq_w = {});*/
		test_mpc.update_mpc(x0, u_fb, x_ref, u_ref, distur_ref, q, r, {2.3}, {2.1});
		temp_matrix = Eigen::MatrixXd(test_mpc.hessian());
		std::cout << prefix + " hessian update1 is :" << std::endl;
		std::cout << temp_matrix << std::endl;
		eigen_to_csv(prefix + "_hessian_update1.csv", temp_matrix);
		//test defualt gradient
		std::cout << prefix + " gradient_update1 is:" << std::endl;
		std::cout << test_mpc.gradient().transpose() << std::endl;
		eigen_to_csv(prefix + "_gradient_update1.csv", test_mpc.gradient());
		temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
		std::cout << prefix + " constriant_matrix update1 is ok:" << std::endl;
		eigen_to_csv(prefix + "_constriant_matrix_update1.csv", test_mpc.constraint_matrix());
		std::cout << temp_matrix << std::endl;
		std::cout << prefix + " update1 bound is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_lowerBound_update1.csv", test_mpc.lowerBound());
		std::cout << test_mpc.upperBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_upperBound_update1.csv", test_mpc.upperBound());

		temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
		std::cout << prefix + " constriant_matrix affter update1  is:" << std::endl;
		std::cout << temp_matrix << std::endl;
		eigen_to_csv(prefix + "_constriant_matrix_update1.csv", temp_matrix);
		std::cout << prefix + " bound affter affter update1 is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_lowerBound_update1.csv", test_mpc.lowerBound());
		std::cout << test_mpc.upperBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_upperBound_update1.csv", test_mpc.upperBound());
		test_mpc.mpc_solve();
	}
	///////////////////////////////////////////////////////////////////////////////////////////////
	{
		//add x_final ae constriant
		std::string prefix = "case4";
		int state_dim = 4;
		int control_dim = 3;
		int distur_dim = 2;
		int mpc_windows = 3;
		int max_xu_constraint = 2;
		int max_final_constraint = 2;
		std::shared_ptr<DiscreteLinearSys> dis_sys_ptr(new DiscreteLinearSys(state_dim, control_dim, distur_dim));

		Eigen::MatrixXd state_matrix(state_dim, state_dim);
		state_matrix << 1.1, 1.2, 1.3, 1.4,
			1.5, 1.6, 1.7, 1.8,
			2.1, 2.2, 2.3, 2.4,
			2.5, 2.6, 2.7, 2.8;

		dis_sys_ptr->set_state_matrix(state_matrix);
		std::cout << "state matrix is:" << std::endl;
		std::cout << state_matrix << std::endl;
		Eigen::MatrixXd control_matrix(state_dim, control_dim);
		control_matrix << 3.1, 3.2, 3.3,
			3.5, 3.6, 3.7,
			4.1, 4.1, 4.3,
			4.5, 4.6, 4.7;
		std::cout << "control matrix is:" << std::endl;
		std::cout << control_matrix << std::endl;
		dis_sys_ptr->set_control_matrix(control_matrix);

		Eigen::MatrixXd distur_matrix(state_dim, distur_dim);
		distur_matrix << 0.1, 0.2,
			0.2, 0.3,
			0.4, 0.5,
			0.6, 0.7;
		dis_sys_ptr->set_distur_matrix(distur_matrix);
		std::cout << "distur_matrix is:" << std::endl;
		std::cout << distur_matrix << std::endl;
		//Mpc(std::shared_ptr<DiscreteLinearSys> dis_sys_ptr, unsigned int mpc_windows,
		/*Mpc(std::shared_ptr<DiscreteLinearSys> dis_sys_ptr,
		unsigned int mpc_windows,
		unsigned int max_xu_constraint,
		unsigned int max_delta_x,
		unsigned int max_delta_u,
		unsigned int max_final_constraint,
		unsigned int max_xu_ae = 0,
		unsigned int max_xu_slack_ineq = 0,
		unsigned int max_x_final_ae = 0,
		unsigned int max_x_final_slack_ineq = 0,
		unsigned int max_delta_u_ae = 0,
		unsigned int max_delta_u_slack_ineq = 0,
		unsigned int max_delta_x_ae = 0,
		unsigned int max_delta_x_slack_ineq = 0);*/
		Mpc test_mpc(dis_sys_ptr, mpc_windows, max_xu_constraint, 1, 1, max_final_constraint, 1, 1, 1);

		//test defualt hessian
		Eigen::MatrixXd temp_matrix = Eigen::MatrixXd(test_mpc.hessian());

		std::cout << prefix + " defualt hessian set is :" << std::endl;
		std::cout << temp_matrix << std::endl;
		eigen_to_csv(prefix + "_defualt_hessian.csv", temp_matrix);

		//test defualt gradient
		std::cout
			<< "prefix defualt gradient is:" << std::endl;
		std::cout << test_mpc.gradient().transpose() << std::endl;
		eigen_to_csv(prefix + "_defualt_gradient.csv", test_mpc.gradient());
		//test defualt constraint matrix

		temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
		std::cout << prefix + " defualt constriant_matrix is ok:" << std::endl;
		eigen_to_csv(prefix + "_defualt_constriant_matrix.csv", test_mpc.constraint_matrix());
		std::cout << temp_matrix << std::endl;
		std::cout << prefix + " default bound is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_defualt_lowerBound.csv", test_mpc.lowerBound());
		std::cout << test_mpc.upperBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_defualt_upperBound.csv", test_mpc.upperBound());
		test_mpc.set_xu_affine({0, 2}, {1.0}, {}, {}, 0, 0, 0);
		/*int Mpc::set_xu_affine(const std::vector<double> x_index, const std::vector<double> x_factor,
	const std::vector<double> u_index, const std::vector<double> u_factor,
	int constraint_index, double min, double max)*/
		test_mpc.set_xu_affine({0, 2}, {1.0, 2.0}, {1, 2}, {1.0, 1.0}, 0, -1, 1);
		test_mpc.set_xu_affine({1, 3}, {1.1, 2.1}, {0, 1}, {1.2, 1.2}, 1, -2, 2);
		//int set_delta_u(unsigned int constraint_index, unsigned int u_index, double min, double max);
		test_mpc.set_delta_u(0, 1, -1000.0, 1000.0);
		test_mpc.set_delta_x(0, 0, -1000.0, 1000.0);
		//int set_final_constraint(int constraint_index, const std::vector<double> x_index, const std::vector<double> x_factor,
		//						 double min, double max);
		test_mpc.set_final_constraint(0, {0, 1, 2, 3}, {0.1, 0.2, 0.3, 0.4}, -0.5, 0.5);
		test_mpc.set_final_constraint(1, {0, 1, 2, 3}, {0.1, 0.3, 0.5, 0.7}, -1.0, 1.0);
		temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
		std::cout << prefix + " constriant_matrix affter add  xu_affine 0 is:" << std::endl;
		std::cout << temp_matrix << std::endl;
		eigen_to_csv(prefix + " constriant_matrix_add_affine0.csv", temp_matrix);
		std::cout << prefix + " bound affter add  xu_affine 0 is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_lowerBound_add_affine0.csv", test_mpc.lowerBound());
		std::cout << test_mpc.upperBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_upperBound_add_affine0.csv", test_mpc.upperBound());
		//set u-1
		Eigen::VectorXd x0(state_dim);
		x0 << 1.1, 1.2, 1.3, 1.4;
		Eigen::VectorXd distur_ref(distur_dim);
		distur_ref << 1.0, 2.0;
		Eigen::MatrixXd q(state_dim, state_dim);
		q << 1.1, 0, 0, 0,
			0, 1.2, 0, 0,
			0, 0, 1.3, 0,
			0, 0, 0, 1.4;
		Eigen::MatrixXd r(control_dim, control_dim);
		r << 1.8, 0, 0,
			0, 1.9, 0,
			0, 0, 1.5;
		Eigen::VectorXd x_ref(state_dim);
		x_ref << 1.0, 2.0, 3.0, 4.0;
		Eigen::VectorXd u_ref(control_dim);
		u_ref << 1.0, 2.0, 3.0;
		Eigen::VectorXd u_fb(3);
		u_fb << 0.1, 0.2, 0.3;

		//	int set_xu_ae(const std::vector<double> x_index, const std::vector<double> x_factor,
		//const std::vector<double> u_index, const std::vector<double> u_factor,
		//int constraint_index, double offset = 0.0);

		test_mpc.set_xu_ae({1, 3}, {0.1, 0.2}, {1, 2}, {0.5, 0.6}, 0, 0.1);
		test_mpc.set_xu_ae_offset(0, 0.2);
		/*int set_xu_slack_ineq(const std::vector<double> x_index, const std::vector<double> x_factor,
						  const std::vector<double> u_index, const std::vector<double> u_factor,
						  int constraint_index, double min, double max);*/
		test_mpc.set_xu_slack_ineq({1, 3}, {0.3, 0.4}, {1, 2}, {0.7, 0.8}, 0, -1000, 1000);
		test_mpc.set_xu_slack_bound(0, -2000, 2000);
		//	int set_x_final_ae(int constraint_index, const std::vector<double> x_index,
		//			   const std::vector<double> x_factor, double offset);
		test_mpc.set_x_final_ae(0, {1}, {1.12}, 0.1);
		/*	bool update_mpc(const Eigen::VectorXd &x0, const std::vector<double> &u_fb,
			const Eigen::MatrixXd &x_ref, const Eigen::MatrixXd &u_ref, const Eigen::MatrixXd &distur_ref,
			const Eigen::MatrixXd &q, const Eigen::MatrixXd &r, const std::vector<double> &xu_ae_w = {},
			const std::vector<double> &xu_slack_ineq_w = {}, const std::vector<double> &x_final_ae_w = {},
			const std::vector<double> &x_final_slack_ineq_w = {}, const std::vector<double> &delta_u_ae_w = {},
			const std::vector<double> &delta_u_slack_ineq_w = {}, const std::vector<double> &delta_x_ae_w = {},
			const std::vector<double> &delta_x_slack_ineq_w = {});*/
		test_mpc.update_mpc(x0, u_fb, x_ref, u_ref, distur_ref, q, r, {2.3}, {2.1}, {2.0});
		temp_matrix = Eigen::MatrixXd(test_mpc.hessian());
		std::cout << prefix + " hessian update1 is :" << std::endl;
		std::cout << temp_matrix << std::endl;
		eigen_to_csv(prefix + "_hessian_update1.csv", temp_matrix);
		//test defualt gradient
		std::cout << prefix + " gradient_update1 is:" << std::endl;
		std::cout << test_mpc.gradient().transpose() << std::endl;
		eigen_to_csv(prefix + "_gradient_update1.csv", test_mpc.gradient());
		temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
		std::cout << prefix + " constriant_matrix update1 is ok:" << std::endl;
		eigen_to_csv(prefix + "_constriant_matrix_update1.csv", test_mpc.constraint_matrix());
		std::cout << temp_matrix << std::endl;
		std::cout << prefix + " update1 bound is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_lowerBound_update1.csv", test_mpc.lowerBound());
		std::cout << test_mpc.upperBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_upperBound_update1.csv", test_mpc.upperBound());

		temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
		std::cout << prefix + " constriant_matrix affter update1  is:" << std::endl;
		std::cout << temp_matrix << std::endl;
		eigen_to_csv(prefix + "_constriant_matrix_update1.csv", temp_matrix);
		std::cout << prefix + " bound affter affter update1 is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_lowerBound_update1.csv", test_mpc.lowerBound());
		std::cout << test_mpc.upperBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_upperBound_update1.csv", test_mpc.upperBound());
		test_mpc.mpc_solve();
	}
	///////////////////////////////////////////////////////////////////////////////////////////////
	{
		//add max_x_final_slack_ineq constriant
		std::string prefix = "case5";
		int state_dim = 4;
		int control_dim = 3;
		int distur_dim = 2;
		int mpc_windows = 3;
		int max_xu_constraint = 2;
		int max_final_constraint = 2;
		std::shared_ptr<DiscreteLinearSys> dis_sys_ptr(new DiscreteLinearSys(state_dim, control_dim, distur_dim));

		Eigen::MatrixXd state_matrix(state_dim, state_dim);
		state_matrix << 1.1, 1.2, 1.3, 1.4,
			1.5, 1.6, 1.7, 1.8,
			2.1, 2.2, 2.3, 2.4,
			2.5, 2.6, 2.7, 2.8;

		dis_sys_ptr->set_state_matrix(state_matrix);
		std::cout << "state matrix is:" << std::endl;
		std::cout << state_matrix << std::endl;
		Eigen::MatrixXd control_matrix(state_dim, control_dim);
		control_matrix << 3.1, 3.2, 3.3,
			3.5, 3.6, 3.7,
			4.1, 4.1, 4.3,
			4.5, 4.6, 4.7;
		std::cout << "control matrix is:" << std::endl;
		std::cout << control_matrix << std::endl;
		dis_sys_ptr->set_control_matrix(control_matrix);

		Eigen::MatrixXd distur_matrix(state_dim, distur_dim);
		distur_matrix << 0.1, 0.2,
			0.2, 0.3,
			0.4, 0.5,
			0.6, 0.7;
		dis_sys_ptr->set_distur_matrix(distur_matrix);
		std::cout << "distur_matrix is:" << std::endl;
		std::cout << distur_matrix << std::endl;
		//Mpc(std::shared_ptr<DiscreteLinearSys> dis_sys_ptr, unsigned int mpc_windows,
		/*Mpc(std::shared_ptr<DiscreteLinearSys> dis_sys_ptr,
		unsigned int mpc_windows,
		unsigned int max_xu_constraint,
		unsigned int max_delta_x,
		unsigned int max_delta_u,
		unsigned int max_final_constraint,
		unsigned int max_xu_ae = 0,
		unsigned int max_xu_slack_ineq = 0,
		unsigned int max_x_final_ae = 0,
		unsigned int max_x_final_slack_ineq = 0,
		unsigned int max_delta_u_ae = 0,
		unsigned int max_delta_u_slack_ineq = 0,
		unsigned int max_delta_x_ae = 0,
		unsigned int max_delta_x_slack_ineq = 0);*/
		Mpc test_mpc(dis_sys_ptr, mpc_windows, max_xu_constraint, 1, 1, max_final_constraint, 1, 1, 1, 1);

		//test defualt hessian
		Eigen::MatrixXd temp_matrix = Eigen::MatrixXd(test_mpc.hessian());

		std::cout << prefix + " defualt hessian set is :" << std::endl;
		std::cout << temp_matrix << std::endl;
		eigen_to_csv(prefix + "_defualt_hessian.csv", temp_matrix);

		//test defualt gradient
		std::cout
			<< "prefix defualt gradient is:" << std::endl;
		std::cout << test_mpc.gradient().transpose() << std::endl;
		eigen_to_csv(prefix + "_defualt_gradient.csv", test_mpc.gradient());
		//test defualt constraint matrix

		temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
		std::cout << prefix + " defualt constriant_matrix is ok:" << std::endl;
		eigen_to_csv(prefix + "_defualt_constriant_matrix.csv", test_mpc.constraint_matrix());
		std::cout << temp_matrix << std::endl;
		std::cout << prefix + " default bound is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_defualt_lowerBound.csv", test_mpc.lowerBound());
		std::cout << test_mpc.upperBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_defualt_upperBound.csv", test_mpc.upperBound());
		test_mpc.set_xu_affine({0, 2}, {1.0}, {}, {}, 0, 0, 0);
		/*int Mpc::set_xu_affine(const std::vector<double> x_index, const std::vector<double> x_factor,
	const std::vector<double> u_index, const std::vector<double> u_factor,
	int constraint_index, double min, double max)*/
		test_mpc.set_xu_affine({0, 2}, {1.0, 2.0}, {1, 2}, {1.0, 1.0}, 0, -1, 1);
		test_mpc.set_xu_affine({1, 3}, {1.1, 2.1}, {0, 1}, {1.2, 1.2}, 1, -2, 2);
		//int set_delta_u(unsigned int constraint_index, unsigned int u_index, double min, double max);
		test_mpc.set_delta_u(0, 1, -1000.0, 1000.0);
		test_mpc.set_delta_x(0, 0, -1000.0, 1000.0);
		//int set_final_constraint(int constraint_index, const std::vector<double> x_index, const std::vector<double> x_factor,
		//						 double min, double max);
		test_mpc.set_final_constraint(0, {0, 1, 2, 3}, {0.1, 0.2, 0.3, 0.4}, -0.5, 0.5);
		test_mpc.set_final_constraint(1, {0, 1, 2, 3}, {0.1, 0.3, 0.5, 0.7}, -1.0, 1.0);
		temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
		std::cout << prefix + " constriant_matrix affter add  xu_affine 0 is:" << std::endl;
		std::cout << temp_matrix << std::endl;
		eigen_to_csv(prefix + " constriant_matrix_add_affine0.csv", temp_matrix);
		std::cout << prefix + " bound affter add  xu_affine 0 is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_lowerBound_add_affine0.csv", test_mpc.lowerBound());
		std::cout << test_mpc.upperBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_upperBound_add_affine0.csv", test_mpc.upperBound());
		//set u-1
		Eigen::VectorXd x0(state_dim);
		x0 << 1.1, 1.2, 1.3, 1.4;
		Eigen::VectorXd distur_ref(distur_dim);
		distur_ref << 1.0, 2.0;
		Eigen::MatrixXd q(state_dim, state_dim);
		q << 1.1, 0, 0, 0,
			0, 1.2, 0, 0,
			0, 0, 1.3, 0,
			0, 0, 0, 1.4;
		Eigen::MatrixXd r(control_dim, control_dim);
		r << 1.8, 0, 0,
			0, 1.9, 0,
			0, 0, 1.5;
		Eigen::VectorXd x_ref(state_dim);
		x_ref << 1.0, 2.0, 3.0, 4.0;
		Eigen::VectorXd u_ref(control_dim);
		u_ref << 1.0, 2.0, 3.0;
		Eigen::VectorXd u_fb(3);
		u_fb << 0.1, 0.2, 0.3;
		//	int set_xu_ae(const std::vector<double> x_index, const std::vector<double> x_factor,
		//const std::vector<double> u_index, const std::vector<double> u_factor,
		//int constraint_index, double offset = 0.0);

		test_mpc.set_xu_ae({1, 3}, {0.1, 0.2}, {1, 2}, {0.5, 0.6}, 0, 0.1);
		test_mpc.set_xu_ae_offset(0, 0.2);
		/*int set_xu_slack_ineq(const std::vector<double> x_index, const std::vector<double> x_factor,
						  const std::vector<double> u_index, const std::vector<double> u_factor,
						  int constraint_index, double min, double max);*/
		test_mpc.set_xu_slack_ineq({1, 3}, {0.3, 0.4}, {1, 2}, {0.7, 0.8}, 0, -1000, 1000);
		test_mpc.set_xu_slack_bound(0, -2000, 2000);
		//	int set_x_final_ae(int constraint_index, const std::vector<double> x_index,
		//			   const std::vector<double> x_factor, double offset);
		test_mpc.set_x_final_ae(0, {1}, {1.12}, 0.1);
		//	int set_x_final_slack_ineq(int constraint_index, const std::vector<double> x_index,
		//					   const std::vector<double> x_factor, double min, double max);
		test_mpc.set_x_final_slack_ineq(0, {1}, {1.13}, -0.11, 0.12);
		test_mpc.set_x_final_slack_ineq_bound(0, -0.21, 0.22);
		/*	bool update_mpc(const Eigen::VectorXd &x0, const std::vector<double> &u_fb,
			const Eigen::MatrixXd &x_ref, const Eigen::MatrixXd &u_ref, const Eigen::MatrixXd &distur_ref,
			const Eigen::MatrixXd &q, const Eigen::MatrixXd &r, const std::vector<double> &xu_ae_w = {},
			const std::vector<double> &xu_slack_ineq_w = {}, const std::vector<double> &x_final_ae_w = {},
			const std::vector<double> &x_final_slack_ineq_w = {}, const std::vector<double> &delta_u_ae_w = {},
			const std::vector<double> &delta_u_slack_ineq_w = {}, const std::vector<double> &delta_x_ae_w = {},
			const std::vector<double> &delta_x_slack_ineq_w = {});*/
		test_mpc.update_mpc(x0, u_fb, x_ref, u_ref, distur_ref, q, r, {2.3}, {2.1}, {2.0}, {4.0});
		temp_matrix = Eigen::MatrixXd(test_mpc.hessian());
		std::cout << prefix + " hessian update1 is :" << std::endl;
		std::cout << temp_matrix << std::endl;
		eigen_to_csv(prefix + "_hessian_update1.csv", temp_matrix);
		//test defualt gradient
		std::cout << prefix + " gradient_update1 is:" << std::endl;
		std::cout << test_mpc.gradient().transpose() << std::endl;
		eigen_to_csv(prefix + "_gradient_update1.csv", test_mpc.gradient());
		temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
		std::cout << prefix + " constriant_matrix update1 is ok:" << std::endl;
		eigen_to_csv(prefix + "_constriant_matrix_update1.csv", test_mpc.constraint_matrix());
		std::cout << temp_matrix << std::endl;
		std::cout << prefix + " update1 bound is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_lowerBound_update1.csv", test_mpc.lowerBound());
		std::cout << test_mpc.upperBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_upperBound_update1.csv", test_mpc.upperBound());

		temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
		std::cout << prefix + " constriant_matrix affter update1  is:" << std::endl;
		std::cout << temp_matrix << std::endl;
		eigen_to_csv(prefix + "_constriant_matrix_update1.csv", temp_matrix);
		std::cout << prefix + " bound affter affter update1 is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_lowerBound_update1.csv", test_mpc.lowerBound());
		std::cout << test_mpc.upperBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_upperBound_update1.csv", test_mpc.upperBound());
		test_mpc.mpc_solve();
	}
	///////////////////////////////////////////////////////////////////////////////////////////////
	{
		//add delta_u_ae constriant
		std::string prefix = "case6";
		int state_dim = 4;
		int control_dim = 3;
		int distur_dim = 2;
		int mpc_windows = 3;
		int max_xu_constraint = 2;
		int max_final_constraint = 2;
		std::shared_ptr<DiscreteLinearSys> dis_sys_ptr(new DiscreteLinearSys(state_dim, control_dim, distur_dim));

		Eigen::MatrixXd state_matrix(state_dim, state_dim);
		state_matrix << 1.1, 1.2, 1.3, 1.4,
			1.5, 1.6, 1.7, 1.8,
			2.1, 2.2, 2.3, 2.4,
			2.5, 2.6, 2.7, 2.8;

		dis_sys_ptr->set_state_matrix(state_matrix);
		std::cout << "state matrix is:" << std::endl;
		std::cout << state_matrix << std::endl;
		Eigen::MatrixXd control_matrix(state_dim, control_dim);
		control_matrix << 3.1, 3.2, 3.3,
			3.5, 3.6, 3.7,
			4.1, 4.1, 4.3,
			4.5, 4.6, 4.7;
		std::cout << "control matrix is:" << std::endl;
		std::cout << control_matrix << std::endl;
		dis_sys_ptr->set_control_matrix(control_matrix);

		Eigen::MatrixXd distur_matrix(state_dim, distur_dim);
		distur_matrix << 0.1, 0.2,
			0.2, 0.3,
			0.4, 0.5,
			0.6, 0.7;
		dis_sys_ptr->set_distur_matrix(distur_matrix);
		std::cout << "distur_matrix is:" << std::endl;
		std::cout << distur_matrix << std::endl;
		//Mpc(std::shared_ptr<DiscreteLinearSys> dis_sys_ptr, unsigned int mpc_windows,
		/*Mpc(std::shared_ptr<DiscreteLinearSys> dis_sys_ptr,
		unsigned int mpc_windows,
		unsigned int max_xu_constraint,
		unsigned int max_delta_x,
		unsigned int max_delta_u,
		unsigned int max_final_constraint,
		unsigned int max_xu_ae = 0,
		unsigned int max_xu_slack_ineq = 0,
		unsigned int max_x_final_ae = 0,
		unsigned int max_x_final_slack_ineq = 0,
		unsigned int max_delta_u_ae = 0,
		unsigned int max_delta_u_slack_ineq = 0,
		unsigned int max_delta_x_ae = 0,
		unsigned int max_delta_x_slack_ineq = 0);*/
		Mpc test_mpc(dis_sys_ptr, mpc_windows, max_xu_constraint, 1, 1, max_final_constraint, 1, 1, 1, 1, 1);

		//test defualt hessian
		Eigen::MatrixXd temp_matrix = Eigen::MatrixXd(test_mpc.hessian());

		std::cout << prefix + " defualt hessian set is :" << std::endl;
		std::cout << temp_matrix << std::endl;
		eigen_to_csv(prefix + "_defualt_hessian.csv", temp_matrix);

		//test defualt gradient
		std::cout
			<< "prefix defualt gradient is:" << std::endl;
		std::cout << test_mpc.gradient().transpose() << std::endl;
		eigen_to_csv(prefix + "_defualt_gradient.csv", test_mpc.gradient());
		//test defualt constraint matrix

		temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
		std::cout << prefix + " defualt constriant_matrix is ok:" << std::endl;
		eigen_to_csv(prefix + "_defualt_constriant_matrix.csv", test_mpc.constraint_matrix());
		std::cout << temp_matrix << std::endl;
		std::cout << prefix + " default bound is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_defualt_lowerBound.csv", test_mpc.lowerBound());
		std::cout << test_mpc.upperBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_defualt_upperBound.csv", test_mpc.upperBound());
		test_mpc.set_xu_affine({0, 2}, {1.0}, {}, {}, 0, 0, 0);
		/*int Mpc::set_xu_affine(const std::vector<double> x_index, const std::vector<double> x_factor,
	const std::vector<double> u_index, const std::vector<double> u_factor,
	int constraint_index, double min, double max)*/
		test_mpc.set_xu_affine({0, 2}, {1.0, 2.0}, {1, 2}, {1.0, 1.0}, 0, -1, 1);
		test_mpc.set_xu_affine({1, 3}, {1.1, 2.1}, {0, 1}, {1.2, 1.2}, 1, -2, 2);
		//int set_delta_u(unsigned int constraint_index, unsigned int u_index, double min, double max);
		test_mpc.set_delta_u(0, 1, -1000.0, 1000.0);
		test_mpc.set_delta_x(0, 0, -1000.0, 1000.0);
		//int set_final_constraint(int constraint_index, const std::vector<double> x_index, const std::vector<double> x_factor,
		//						 double min, double max);
		test_mpc.set_final_constraint(0, {0, 1, 2, 3}, {0.1, 0.2, 0.3, 0.4}, -0.5, 0.5);
		test_mpc.set_final_constraint(1, {0, 1, 2, 3}, {0.1, 0.3, 0.5, 0.7}, -1.0, 1.0);
		temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
		std::cout << prefix + " constriant_matrix affter add  xu_affine 0 is:" << std::endl;
		std::cout << temp_matrix << std::endl;
		eigen_to_csv(prefix + " constriant_matrix_add_affine0.csv", temp_matrix);
		std::cout << prefix + " bound affter add  xu_affine 0 is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_lowerBound_add_affine0.csv", test_mpc.lowerBound());
		std::cout << test_mpc.upperBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_upperBound_add_affine0.csv", test_mpc.upperBound());
		//set u-1
		Eigen::VectorXd x0(state_dim);
		x0 << 1.1, 1.2, 1.3, 1.4;
		Eigen::VectorXd distur_ref(distur_dim);
		distur_ref << 1.0, 2.0;
		Eigen::MatrixXd q(state_dim, state_dim);
		q << 1.1, 0, 0, 0,
			0, 1.2, 0, 0,
			0, 0, 1.3, 0,
			0, 0, 0, 1.4;
		Eigen::MatrixXd r(control_dim, control_dim);
		r << 1.8, 0, 0,
			0, 1.9, 0,
			0, 0, 1.5;
		Eigen::VectorXd x_ref(state_dim);
		x_ref << 1.0, 2.0, 3.0, 4.0;
		Eigen::VectorXd u_ref(control_dim);
		u_ref << 1.0, 2.0, 3.0;
		Eigen::VectorXd u_fb(3);
		u_fb << 0.1, 0.2, 0.3;
		//	int set_xu_ae(const std::vector<double> x_index, const std::vector<double> x_factor,
		//const std::vector<double> u_index, const std::vector<double> u_factor,
		//int constraint_index, double offset = 0.0);

		test_mpc.set_xu_ae({1, 3}, {0.1, 0.2}, {1, 2}, {0.5, 0.6}, 0, 0.1);
		test_mpc.set_xu_ae_offset(0, 0.2);
		/*int set_xu_slack_ineq(const std::vector<double> x_index, const std::vector<double> x_factor,
						  const std::vector<double> u_index, const std::vector<double> u_factor,
						  int constraint_index, double min, double max);*/
		test_mpc.set_xu_slack_ineq({1, 3}, {0.3, 0.4}, {1, 2}, {0.7, 0.8}, 0, -1000, 1000);
		test_mpc.set_xu_slack_bound(0, -2000, 2000);
		//	int set_x_final_ae(int constraint_index, const std::vector<double> x_index,
		//			   const std::vector<double> x_factor, double offset);
		test_mpc.set_x_final_ae(0, {1}, {1.12}, 0.1);
		//	int set_x_final_slack_ineq(int constraint_index, const std::vector<double> x_index,
		//					   const std::vector<double> x_factor, double min, double max);
		test_mpc.set_x_final_slack_ineq(0, {1}, {1.13}, -0.11, 0.12);
		test_mpc.set_x_final_slack_ineq_bound(0, -0.21, 0.22);
		//int set_delta_u_ae(unsigned int constraint_index, unsigned int control_index, double offset);
		test_mpc.set_delta_u_ae(0, 0, 0);
		//int set_delta_u_ae_offset(unsigned int constraint_index, double offset);
		test_mpc.set_delta_u_ae_offset(0, 0.1);
		/*	bool update_mpc(const Eigen::VectorXd &x0, const std::vector<double> &u_fb,
			const Eigen::MatrixXd &x_ref, const Eigen::MatrixXd &u_ref, const Eigen::MatrixXd &distur_ref,
			const Eigen::MatrixXd &q, const Eigen::MatrixXd &r, const std::vector<double> &xu_ae_w = {},
			const std::vector<double> &xu_slack_ineq_w = {}, const std::vector<double> &x_final_ae_w = {},
			const std::vector<double> &x_final_slack_ineq_w = {}, const std::vector<double> &delta_u_ae_w = {},
			const std::vector<double> &delta_u_slack_ineq_w = {}, const std::vector<double> &delta_x_ae_w = {},
			const std::vector<double> &delta_x_slack_ineq_w = {});*/
		test_mpc.update_mpc(x0, u_fb, x_ref, u_ref, distur_ref, q, r, {2.3}, {2.1}, {2.0}, {4.0}, {3.1});
		temp_matrix = Eigen::MatrixXd(test_mpc.hessian());
		std::cout << prefix + " hessian update1 is :" << std::endl;
		std::cout << temp_matrix << std::endl;
		eigen_to_csv(prefix + "_hessian_update1.csv", temp_matrix);
		//test defualt gradient
		std::cout << prefix + " gradient_update1 is:" << std::endl;
		std::cout << test_mpc.gradient().transpose() << std::endl;
		eigen_to_csv(prefix + "_gradient_update1.csv", test_mpc.gradient());
		temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
		std::cout << prefix + " constriant_matrix update1 is ok:" << std::endl;
		eigen_to_csv(prefix + "_constriant_matrix_update1.csv", test_mpc.constraint_matrix());
		std::cout << temp_matrix << std::endl;
		std::cout << prefix + " update1 bound is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_lowerBound_update1.csv", test_mpc.lowerBound());
		std::cout << test_mpc.upperBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_upperBound_update1.csv", test_mpc.upperBound());

		temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
		std::cout << prefix + " constriant_matrix affter update1  is:" << std::endl;
		std::cout << temp_matrix << std::endl;
		eigen_to_csv(prefix + "_constriant_matrix_update1.csv", temp_matrix);
		std::cout << prefix + " bound affter affter update1 is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_lowerBound_update1.csv", test_mpc.lowerBound());
		std::cout << test_mpc.upperBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_upperBound_update1.csv", test_mpc.upperBound());
		test_mpc.mpc_solve();
	}
	///////////////////////////////////////////////////////////////////////////////////////////////
	{
		//add delta_u_slack_ineq constriant
		std::string prefix = "case7";
		int state_dim = 4;
		int control_dim = 3;
		int distur_dim = 2;
		int mpc_windows = 3;
		int max_xu_constraint = 2;
		int max_final_constraint = 2;
		std::shared_ptr<DiscreteLinearSys> dis_sys_ptr(new DiscreteLinearSys(state_dim, control_dim, distur_dim));

		Eigen::MatrixXd state_matrix(state_dim, state_dim);
		state_matrix << 1.1, 1.2, 1.3, 1.4,
			1.5, 1.6, 1.7, 1.8,
			2.1, 2.2, 2.3, 2.4,
			2.5, 2.6, 2.7, 2.8;

		dis_sys_ptr->set_state_matrix(state_matrix);
		std::cout << "state matrix is:" << std::endl;
		std::cout << state_matrix << std::endl;
		Eigen::MatrixXd control_matrix(state_dim, control_dim);
		control_matrix << 3.1, 3.2, 3.3,
			3.5, 3.6, 3.7,
			4.1, 4.1, 4.3,
			4.5, 4.6, 4.7;
		std::cout << "control matrix is:" << std::endl;
		std::cout << control_matrix << std::endl;
		dis_sys_ptr->set_control_matrix(control_matrix);

		Eigen::MatrixXd distur_matrix(state_dim, distur_dim);
		distur_matrix << 0.1, 0.2,
			0.2, 0.3,
			0.4, 0.5,
			0.6, 0.7;
		dis_sys_ptr->set_distur_matrix(distur_matrix);
		std::cout << "distur_matrix is:" << std::endl;
		std::cout << distur_matrix << std::endl;
		//Mpc(std::shared_ptr<DiscreteLinearSys> dis_sys_ptr, unsigned int mpc_windows,
		/*Mpc(std::shared_ptr<DiscreteLinearSys> dis_sys_ptr,
		unsigned int mpc_windows,
		unsigned int max_xu_constraint,
		unsigned int max_delta_x,
		unsigned int max_delta_u,
		unsigned int max_final_constraint,
		unsigned int max_xu_ae = 0,
		unsigned int max_xu_slack_ineq = 0,
		unsigned int max_x_final_ae = 0,
		unsigned int max_x_final_slack_ineq = 0,
		unsigned int max_delta_u_ae = 0,
		unsigned int max_delta_u_slack_ineq = 0,
		unsigned int max_delta_x_ae = 0,
		unsigned int max_delta_x_slack_ineq = 0);*/
		Mpc test_mpc(dis_sys_ptr, mpc_windows, max_xu_constraint, 1, 1, max_final_constraint, 1, 1, 1, 1, 1, 1);

		//test defualt hessian
		Eigen::MatrixXd temp_matrix = Eigen::MatrixXd(test_mpc.hessian());

		std::cout << prefix + " defualt hessian set is :" << std::endl;
		std::cout << temp_matrix << std::endl;
		eigen_to_csv(prefix + "_defualt_hessian.csv", temp_matrix);

		//test defualt gradient
		std::cout
			<< "prefix defualt gradient is:" << std::endl;
		std::cout << test_mpc.gradient().transpose() << std::endl;
		eigen_to_csv(prefix + "_defualt_gradient.csv", test_mpc.gradient());
		//test defualt constraint matrix

		temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
		std::cout << prefix + " defualt constriant_matrix is ok:" << std::endl;
		eigen_to_csv(prefix + "_defualt_constriant_matrix.csv", test_mpc.constraint_matrix());
		std::cout << temp_matrix << std::endl;
		std::cout << prefix + " default bound is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_defualt_lowerBound.csv", test_mpc.lowerBound());
		std::cout << test_mpc.upperBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_defualt_upperBound.csv", test_mpc.upperBound());
		test_mpc.set_xu_affine({0, 2}, {1.0}, {}, {}, 0, 0, 0);
		/*int Mpc::set_xu_affine(const std::vector<double> x_index, const std::vector<double> x_factor,
	const std::vector<double> u_index, const std::vector<double> u_factor,
	int constraint_index, double min, double max)*/
		test_mpc.set_xu_affine({0, 2}, {1.0, 2.0}, {1, 2}, {1.0, 1.0}, 0, -1, 1);
		test_mpc.set_xu_affine({1, 3}, {1.1, 2.1}, {0, 1}, {1.2, 1.2}, 1, -2, 2);
		//int set_delta_u(unsigned int constraint_index, unsigned int u_index, double min, double max);
		test_mpc.set_delta_u(0, 1, -1000.0, 1000.0);
		test_mpc.set_delta_x(0, 0, -1000.0, 1000.0);
		//int set_final_constraint(int constraint_index, const std::vector<double> x_index, const std::vector<double> x_factor,
		//						 double min, double max);
		test_mpc.set_final_constraint(0, {0, 1, 2, 3}, {0.1, 0.2, 0.3, 0.4}, -0.5, 0.5);
		test_mpc.set_final_constraint(1, {0, 1, 2, 3}, {0.1, 0.3, 0.5, 0.7}, -1.0, 1.0);
		temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
		std::cout << prefix + " constriant_matrix affter add  xu_affine 0 is:" << std::endl;
		std::cout << temp_matrix << std::endl;
		eigen_to_csv(prefix + " constriant_matrix_add_affine0.csv", temp_matrix);
		std::cout << prefix + " bound affter add  xu_affine 0 is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_lowerBound_add_affine0.csv", test_mpc.lowerBound());
		std::cout << test_mpc.upperBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_upperBound_add_affine0.csv", test_mpc.upperBound());
		//set u-1
		Eigen::VectorXd x0(state_dim);
		x0 << 1.1, 1.2, 1.3, 1.4;
		Eigen::VectorXd distur_ref(distur_dim);
		distur_ref << 1.0, 2.0;
		Eigen::MatrixXd q(state_dim, state_dim);
		q << 1.1, 0, 0, 0,
			0, 1.2, 0, 0,
			0, 0, 1.3, 0,
			0, 0, 0, 1.4;
		Eigen::MatrixXd r(control_dim, control_dim);
		r << 1.8, 0, 0,
			0, 1.9, 0,
			0, 0, 1.5;
		Eigen::VectorXd x_ref(state_dim);
		x_ref << 1.0, 2.0, 3.0, 4.0;
		Eigen::VectorXd u_ref(control_dim);
		u_ref << 1.0, 2.0, 3.0;
		Eigen::VectorXd u_fb(3);
		u_fb << 0.1, 0.2, 0.3;
		//	int set_xu_ae(const std::vector<double> x_index, const std::vector<double> x_factor,
		//const std::vector<double> u_index, const std::vector<double> u_factor,
		//int constraint_index, double offset = 0.0);

		test_mpc.set_xu_ae({1, 3}, {0.1, 0.2}, {1, 2}, {0.5, 0.6}, 0, 0.1);
		test_mpc.set_xu_ae_offset(0, 0.2);
		/*int set_xu_slack_ineq(const std::vector<double> x_index, const std::vector<double> x_factor,
						  const std::vector<double> u_index, const std::vector<double> u_factor,
						  int constraint_index, double min, double max);*/
		test_mpc.set_xu_slack_ineq({1, 3}, {0.3, 0.4}, {1, 2}, {0.7, 0.8}, 0, -1000, 1000);
		test_mpc.set_xu_slack_bound(0, -2000, 2000);
		//	int set_x_final_ae(int constraint_index, const std::vector<double> x_index,
		//			   const std::vector<double> x_factor, double offset);
		test_mpc.set_x_final_ae(0, {1}, {1.12}, 0.1);
		//	int set_x_final_slack_ineq(int constraint_index, const std::vector<double> x_index,
		//					   const std::vector<double> x_factor, double min, double max);
		test_mpc.set_x_final_slack_ineq(0, {1}, {1.13}, -0.11, 0.12);
		test_mpc.set_x_final_slack_ineq_bound(0, -0.21, 0.22);
		//int set_delta_u_ae(unsigned int constraint_index, unsigned int control_index, double offset);
		test_mpc.set_delta_u_ae(0, 0, 0);
		//int set_delta_u_ae_offset(unsigned int constraint_index, double offset);
		test_mpc.set_delta_u_ae_offset(0, 0.1);
		//int set_delta_u_slack_ineq(unsigned int constraint_index, unsigned int u_index, double min, double max);
		test_mpc.set_delta_u_slack_ineq(0, 1, -1500, 1500);
		test_mpc.set_delta_u_slack_ineq_bound(0, -1500.1, 1500.1);
		/*	bool update_mpc(const Eigen::VectorXd &x0, const std::vector<double> &u_fb,
			const Eigen::MatrixXd &x_ref, const Eigen::MatrixXd &u_ref, const Eigen::MatrixXd &distur_ref,
			const Eigen::MatrixXd &q, const Eigen::MatrixXd &r, const std::vector<double> &xu_ae_w = {},
			const std::vector<double> &xu_slack_ineq_w = {}, const std::vector<double> &x_final_ae_w = {},
			const std::vector<double> &x_final_slack_ineq_w = {}, const std::vector<double> &delta_u_ae_w = {},
			const std::vector<double> &delta_u_slack_ineq_w = {}, const std::vector<double> &delta_x_ae_w = {},
			const std::vector<double> &delta_x_slack_ineq_w = {});*/
		test_mpc.update_mpc(x0, u_fb, x_ref, u_ref, distur_ref, q, r, {2.3}, {2.1}, {2.0}, {4.0}, {3.1}, {2.1});
		temp_matrix = Eigen::MatrixXd(test_mpc.hessian());
		std::cout << prefix + " hessian update1 is :" << std::endl;
		std::cout << temp_matrix << std::endl;
		eigen_to_csv(prefix + "_hessian_update1.csv", temp_matrix);
		//test defualt gradient
		std::cout << prefix + " gradient_update1 is:" << std::endl;
		std::cout << test_mpc.gradient().transpose() << std::endl;
		eigen_to_csv(prefix + "_gradient_update1.csv", test_mpc.gradient());
		temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
		std::cout << prefix + " constriant_matrix update1 is ok:" << std::endl;
		eigen_to_csv(prefix + "_constriant_matrix_update1.csv", test_mpc.constraint_matrix());
		std::cout << temp_matrix << std::endl;
		std::cout << prefix + " update1 bound is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_lowerBound_update1.csv", test_mpc.lowerBound());
		std::cout << test_mpc.upperBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_upperBound_update1.csv", test_mpc.upperBound());

		temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
		std::cout << prefix + " constriant_matrix affter update1  is:" << std::endl;
		std::cout << temp_matrix << std::endl;
		eigen_to_csv(prefix + "_constriant_matrix_update1.csv", temp_matrix);
		std::cout << prefix + " bound affter affter update1 is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_lowerBound_update1.csv", test_mpc.lowerBound());
		std::cout << test_mpc.upperBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_upperBound_update1.csv", test_mpc.upperBound());
		test_mpc.mpc_solve();
	}
	///////////////////////////////////////////////////////////////////////////////////////////////
	{
		//add delta_u_slack_ineq constriant
		std::string prefix = "case8";
		int state_dim = 4;
		int control_dim = 3;
		int distur_dim = 2;
		int mpc_windows = 3;
		int max_xu_constraint = 2;
		int max_final_constraint = 2;
		std::shared_ptr<DiscreteLinearSys> dis_sys_ptr(new DiscreteLinearSys(state_dim, control_dim, distur_dim));

		Eigen::MatrixXd state_matrix(state_dim, state_dim);
		state_matrix << 1.1, 1.2, 1.3, 1.4,
			1.5, 1.6, 1.7, 1.8,
			2.1, 2.2, 2.3, 2.4,
			2.5, 2.6, 2.7, 2.8;

		dis_sys_ptr->set_state_matrix(state_matrix);
		std::cout << "state matrix is:" << std::endl;
		std::cout << state_matrix << std::endl;
		Eigen::MatrixXd control_matrix(state_dim, control_dim);
		control_matrix << 3.1, 3.2, 3.3,
			3.5, 3.6, 3.7,
			4.1, 4.1, 4.3,
			4.5, 4.6, 4.7;
		std::cout << "control matrix is:" << std::endl;
		std::cout << control_matrix << std::endl;
		dis_sys_ptr->set_control_matrix(control_matrix);

		Eigen::MatrixXd distur_matrix(state_dim, distur_dim);
		distur_matrix << 0.1, 0.2,
			0.2, 0.3,
			0.4, 0.5,
			0.6, 0.7;
		dis_sys_ptr->set_distur_matrix(distur_matrix);
		std::cout << "distur_matrix is:" << std::endl;
		std::cout << distur_matrix << std::endl;
		//Mpc(std::shared_ptr<DiscreteLinearSys> dis_sys_ptr, unsigned int mpc_windows,
		/*Mpc(std::shared_ptr<DiscreteLinearSys> dis_sys_ptr,
		unsigned int mpc_windows,
		unsigned int max_xu_constraint,
		unsigned int max_delta_x,
		unsigned int max_delta_u,
		unsigned int max_final_constraint,
		unsigned int max_xu_ae = 0,
		unsigned int max_xu_slack_ineq = 0,
		unsigned int max_x_final_ae = 0,
		unsigned int max_x_final_slack_ineq = 0,
		unsigned int max_delta_u_ae = 0,
		unsigned int max_delta_u_slack_ineq = 0,
		unsigned int max_delta_x_ae = 0,
		unsigned int max_delta_x_slack_ineq = 0);*/
		Mpc test_mpc(dis_sys_ptr, mpc_windows, max_xu_constraint, 1, 1, max_final_constraint, 1, 1, 1, 1, 1, 1, 1);

		//test defualt hessian
		Eigen::MatrixXd temp_matrix = Eigen::MatrixXd(test_mpc.hessian());

		std::cout << prefix + " defualt hessian set is :" << std::endl;
		std::cout << temp_matrix << std::endl;
		eigen_to_csv(prefix + "_defualt_hessian.csv", temp_matrix);

		//test defualt gradient
		std::cout
			<< "prefix defualt gradient is:" << std::endl;
		std::cout << test_mpc.gradient().transpose() << std::endl;
		eigen_to_csv(prefix + "_defualt_gradient.csv", test_mpc.gradient());
		//test defualt constraint matrix

		temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
		std::cout << prefix + " defualt constriant_matrix is ok:" << std::endl;
		eigen_to_csv(prefix + "_defualt_constriant_matrix.csv", test_mpc.constraint_matrix());
		std::cout << temp_matrix << std::endl;
		std::cout << prefix + " default bound is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_defualt_lowerBound.csv", test_mpc.lowerBound());
		std::cout << test_mpc.upperBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_defualt_upperBound.csv", test_mpc.upperBound());
		test_mpc.set_xu_affine({0, 2}, {1.0}, {}, {}, 0, 0, 0);
		/*int Mpc::set_xu_affine(const std::vector<double> x_index, const std::vector<double> x_factor,
	const std::vector<double> u_index, const std::vector<double> u_factor,
	int constraint_index, double min, double max)*/
		test_mpc.set_xu_affine({0, 2}, {1.0, 2.0}, {1, 2}, {1.0, 1.0}, 0, -1, 1);
		test_mpc.set_xu_affine({1, 3}, {1.1, 2.1}, {0, 1}, {1.2, 1.2}, 1, -2, 2);
		//int set_delta_u(unsigned int constraint_index, unsigned int u_index, double min, double max);
		test_mpc.set_delta_u(0, 1, -1000.0, 1000.0);
		test_mpc.set_delta_x(0, 0, -1000.0, 1000.0);
		//int set_final_constraint(int constraint_index, const std::vector<double> x_index, const std::vector<double> x_factor,
		//						 double min, double max);
		test_mpc.set_final_constraint(0, {0, 1, 2, 3}, {0.1, 0.2, 0.3, 0.4}, -0.5, 0.5);
		test_mpc.set_final_constraint(1, {0, 1, 2, 3}, {0.1, 0.3, 0.5, 0.7}, -1.0, 1.0);
		temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
		std::cout << prefix + " constriant_matrix affter add  xu_affine 0 is:" << std::endl;
		std::cout << temp_matrix << std::endl;
		eigen_to_csv(prefix + " constriant_matrix_add_affine0.csv", temp_matrix);
		std::cout << prefix + " bound affter add  xu_affine 0 is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_lowerBound_add_affine0.csv", test_mpc.lowerBound());
		std::cout << test_mpc.upperBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_upperBound_add_affine0.csv", test_mpc.upperBound());
		//set u-1
		Eigen::VectorXd x0(state_dim);
		x0 << 1.1, 1.2, 1.3, 1.4;
		Eigen::VectorXd distur_ref(distur_dim);
		distur_ref << 1.0, 2.0;
		Eigen::MatrixXd q(state_dim, state_dim);
		q << 1.1, 0, 0, 0,
			0, 1.2, 0, 0,
			0, 0, 1.3, 0,
			0, 0, 0, 1.4;
		Eigen::MatrixXd r(control_dim, control_dim);
		r << 1.8, 0, 0,
			0, 1.9, 0,
			0, 0, 1.5;
		Eigen::VectorXd x_ref(state_dim);
		x_ref << 1.0, 2.0, 3.0, 4.0;
		Eigen::VectorXd u_ref(control_dim);
		u_ref << 1.0, 2.0, 3.0;
		Eigen::VectorXd u_fb(3);
		u_fb << 0.1, 0.2, 0.3;
		//	int set_xu_ae(const std::vector<double> x_index, const std::vector<double> x_factor,
		//const std::vector<double> u_index, const std::vector<double> u_factor,
		//int constraint_index, double offset = 0.0);

		test_mpc.set_xu_ae({1, 3}, {0.1, 0.2}, {1, 2}, {0.5, 0.6}, 0, 0.1);
		test_mpc.set_xu_ae_offset(0, 0.2);
		/*int set_xu_slack_ineq(const std::vector<double> x_index, const std::vector<double> x_factor,
						  const std::vector<double> u_index, const std::vector<double> u_factor,
						  int constraint_index, double min, double max);*/
		test_mpc.set_xu_slack_ineq({1, 3}, {0.3, 0.4}, {1, 2}, {0.7, 0.8}, 0, -1000, 1000);
		test_mpc.set_xu_slack_bound(0, -2000, 2000);
		//	int set_x_final_ae(int constraint_index, const std::vector<double> x_index,
		//			   const std::vector<double> x_factor, double offset);
		test_mpc.set_x_final_ae(0, {1}, {1.12}, 0.1);
		//	int set_x_final_slack_ineq(int constraint_index, const std::vector<double> x_index,
		//					   const std::vector<double> x_factor, double min, double max);
		test_mpc.set_x_final_slack_ineq(0, {1}, {1.13}, -0.11, 0.12);
		test_mpc.set_x_final_slack_ineq_bound(0, -0.21, 0.22);
		//int set_delta_u_ae(unsigned int constraint_index, unsigned int control_index, double offset);
		test_mpc.set_delta_u_ae(0, 0, 0);
		//int set_delta_u_ae_offset(unsigned int constraint_index, double offset);
		test_mpc.set_delta_u_ae_offset(0, 0.1);
		//int set_delta_u_slack_ineq(unsigned int constraint_index, unsigned int u_index, double min, double max);
		test_mpc.set_delta_u_slack_ineq(0, 1, -1500, 1500);
		test_mpc.set_delta_u_slack_ineq_bound(0, -1500.1, 1500.1);
		//int set_delta_x_ae(unsigned int constraint_index, unsigned int x_index, double offset);
		test_mpc.set_delta_x_ae(0, 1, 0.0);
		test_mpc.set_delta_x_ae_offset(0, 0.3);
		/*	bool update_mpc(const Eigen::VectorXd &x0, const std::vector<double> &u_fb,
			const Eigen::MatrixXd &x_ref, const Eigen::MatrixXd &u_ref, const Eigen::MatrixXd &distur_ref,
			const Eigen::MatrixXd &q, const Eigen::MatrixXd &r, const std::vector<double> &xu_ae_w = {},
			const std::vector<double> &xu_slack_ineq_w = {}, const std::vector<double> &x_final_ae_w = {},
			const std::vector<double> &x_final_slack_ineq_w = {}, const std::vector<double> &delta_u_ae_w = {},
			const std::vector<double> &delta_u_slack_ineq_w = {}, const std::vector<double> &delta_x_ae_w = {},
			const std::vector<double> &delta_x_slack_ineq_w = {});*/
		test_mpc.update_mpc(x0, u_fb, x_ref, u_ref, distur_ref, q, r, {2.3}, {2.1}, {2.0}, {4.0}, {3.1}, {2.1}, {1.1});
		temp_matrix = Eigen::MatrixXd(test_mpc.hessian());
		std::cout << prefix + " hessian update1 is :" << std::endl;
		std::cout << temp_matrix << std::endl;
		eigen_to_csv(prefix + "_hessian_update1.csv", temp_matrix);
		//test defualt gradient
		std::cout << prefix + " gradient_update1 is:" << std::endl;
		std::cout << test_mpc.gradient().transpose() << std::endl;
		eigen_to_csv(prefix + "_gradient_update1.csv", test_mpc.gradient());
		temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
		std::cout << prefix + " constriant_matrix update1 is ok:" << std::endl;
		eigen_to_csv(prefix + "_constriant_matrix_update1.csv", test_mpc.constraint_matrix());
		std::cout << temp_matrix << std::endl;
		std::cout << prefix + " update1 bound is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_lowerBound_update1.csv", test_mpc.lowerBound());
		std::cout << test_mpc.upperBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_upperBound_update1.csv", test_mpc.upperBound());

		temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
		std::cout << prefix + " constriant_matrix affter update1  is:" << std::endl;
		std::cout << temp_matrix << std::endl;
		eigen_to_csv(prefix + "_constriant_matrix_update1.csv", temp_matrix);
		std::cout << prefix + " bound affter affter update1 is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_lowerBound_update1.csv", test_mpc.lowerBound());
		std::cout << test_mpc.upperBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_upperBound_update1.csv", test_mpc.upperBound());
		test_mpc.mpc_solve();
	}
	///////////////////////////////////////////////////////////////////////////////////////////////
	{
		//add delta_u_slack_ineq constriant
		std::string prefix = "case9";
		int state_dim = 4;
		int control_dim = 3;
		int distur_dim = 2;
		int mpc_windows = 3;
		int max_xu_constraint = 2;
		int max_final_constraint = 2;
		std::shared_ptr<DiscreteLinearSys> dis_sys_ptr(new DiscreteLinearSys(state_dim, control_dim, distur_dim));

		Eigen::MatrixXd state_matrix(state_dim, state_dim);
		state_matrix << 1.1, 1.2, 1.3, 1.4,
			1.5, 1.6, 1.7, 1.8,
			2.1, 2.2, 2.3, 2.4,
			2.5, 2.6, 2.7, 2.8;

		dis_sys_ptr->set_state_matrix(state_matrix);
		std::cout << "state matrix is:" << std::endl;
		std::cout << state_matrix << std::endl;
		Eigen::MatrixXd control_matrix(state_dim, control_dim);
		control_matrix << 3.1, 3.2, 3.3,
			3.5, 3.6, 3.7,
			4.1, 4.1, 4.3,
			4.5, 4.6, 4.7;
		std::cout << "control matrix is:" << std::endl;
		std::cout << control_matrix << std::endl;
		dis_sys_ptr->set_control_matrix(control_matrix);

		Eigen::MatrixXd distur_matrix(state_dim, distur_dim);
		distur_matrix << 0.1, 0.2,
			0.2, 0.3,
			0.4, 0.5,
			0.6, 0.7;
		dis_sys_ptr->set_distur_matrix(distur_matrix);
		std::cout << "distur_matrix is:" << std::endl;
		std::cout << distur_matrix << std::endl;
		//Mpc(std::shared_ptr<DiscreteLinearSys> dis_sys_ptr, unsigned int mpc_windows,
		/*Mpc(std::shared_ptr<DiscreteLinearSys> dis_sys_ptr,
		unsigned int mpc_windows,
		unsigned int max_xu_constraint,
		unsigned int max_delta_x,
		unsigned int max_delta_u,
		unsigned int max_final_constraint,
		unsigned int max_xu_ae = 0,
		unsigned int max_xu_slack_ineq = 0,
		unsigned int max_x_final_ae = 0,
		unsigned int max_x_final_slack_ineq = 0,
		unsigned int max_delta_u_ae = 0,
		unsigned int max_delta_u_slack_ineq = 0,
		unsigned int max_delta_x_ae = 0,
		unsigned int max_delta_x_slack_ineq = 0);*/
		Mpc test_mpc(dis_sys_ptr, mpc_windows, max_xu_constraint, 1, 1, max_final_constraint, 1, 1, 1, 1, 1, 1, 1, 1);

		//test defualt hessian
		Eigen::MatrixXd temp_matrix = Eigen::MatrixXd(test_mpc.hessian());

		std::cout << prefix + " defualt hessian set is :" << std::endl;
		std::cout << temp_matrix << std::endl;
		eigen_to_csv(prefix + "_defualt_hessian.csv", temp_matrix);

		//test defualt gradient
		std::cout
			<< "prefix defualt gradient is:" << std::endl;
		std::cout << test_mpc.gradient().transpose() << std::endl;
		eigen_to_csv(prefix + "_defualt_gradient.csv", test_mpc.gradient());
		//test defualt constraint matrix

		temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
		std::cout << prefix + " defualt constriant_matrix is ok:" << std::endl;
		eigen_to_csv(prefix + "_defualt_constriant_matrix.csv", test_mpc.constraint_matrix());
		std::cout << temp_matrix << std::endl;
		std::cout << prefix + " default bound is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_defualt_lowerBound.csv", test_mpc.lowerBound());
		std::cout << test_mpc.upperBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_defualt_upperBound.csv", test_mpc.upperBound());
		test_mpc.set_xu_affine({0, 2}, {1.0}, {}, {}, 0, 0, 0);
		/*int Mpc::set_xu_affine(const std::vector<double> x_index, const std::vector<double> x_factor,
	const std::vector<double> u_index, const std::vector<double> u_factor,
	int constraint_index, double min, double max)*/
		test_mpc.set_xu_affine({0, 2}, {1.0, 2.0}, {1, 2}, {1.0, 1.0}, 0, -1, 1);
		test_mpc.set_xu_affine({1, 3}, {1.1, 2.1}, {0, 1}, {1.2, 1.2}, 1, -2, 2);
		//int set_delta_u(unsigned int constraint_index, unsigned int u_index, double min, double max);
		test_mpc.set_delta_u(0, 1, -1000.0, 1000.0);
		test_mpc.set_delta_x(0, 0, -1000.0, 1000.0);
		//int set_final_constraint(int constraint_index, const std::vector<double> x_index, const std::vector<double> x_factor,
		//						 double min, double max);
		test_mpc.set_final_constraint(0, {0, 1, 2, 3}, {0.1, 0.2, 0.3, 0.4}, -0.5, 0.5);
		test_mpc.set_final_constraint(1, {0, 1, 2, 3}, {0.1, 0.3, 0.5, 0.7}, -1.0, 1.0);
		temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
		std::cout << prefix + " constriant_matrix affter add  xu_affine 0 is:" << std::endl;
		std::cout << temp_matrix << std::endl;
		eigen_to_csv(prefix + " constriant_matrix_add_affine0.csv", temp_matrix);
		std::cout << prefix + " bound affter add  xu_affine 0 is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_lowerBound_add_affine0.csv", test_mpc.lowerBound());
		std::cout << test_mpc.upperBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_upperBound_add_affine0.csv", test_mpc.upperBound());
		//set u-1
		Eigen::VectorXd x0(state_dim);
		x0 << 1.1, 1.2, 1.3, 1.4;
		Eigen::VectorXd distur_ref(distur_dim);
		distur_ref << 1.0, 2.0;
		Eigen::MatrixXd q(state_dim, state_dim);
		q << 1.1, 0, 0, 0,
			0, 1.2, 0, 0,
			0, 0, 1.3, 0,
			0, 0, 0, 1.4;
		Eigen::MatrixXd r(control_dim, control_dim);
		r << 1.8, 0, 0,
			0, 1.9, 0,
			0, 0, 1.5;
		Eigen::VectorXd x_ref(state_dim);
		x_ref << 1.0, 2.0, 3.0, 4.0;
		Eigen::VectorXd u_ref(control_dim);
		u_ref << 1.0, 2.0, 3.0;
		Eigen::VectorXd u_fb(3);
		u_fb << 0.1, 0.2, 0.3;
		//	int set_xu_ae(const std::vector<double> x_index, const std::vector<double> x_factor,
		//const std::vector<double> u_index, const std::vector<double> u_factor,
		//int constraint_index, double offset = 0.0);

		test_mpc.set_xu_ae({1, 3}, {0.1, 0.2}, {1, 2}, {0.5, 0.6}, 0, 0.1);
		test_mpc.set_xu_ae_offset(0, 0.2);
		/*int set_xu_slack_ineq(const std::vector<double> x_index, const std::vector<double> x_factor,
						  const std::vector<double> u_index, const std::vector<double> u_factor,
						  int constraint_index, double min, double max);*/
		test_mpc.set_xu_slack_ineq({1, 3}, {0.3, 0.4}, {1, 2}, {0.7, 0.8}, 0, -1000, 1000);
		test_mpc.set_xu_slack_bound(0, -2000, 2000);
		//	int set_x_final_ae(int constraint_index, const std::vector<double> x_index,
		//			   const std::vector<double> x_factor, double offset);
		test_mpc.set_x_final_ae(0, {1}, {1.12}, 0.1);
		//	int set_x_final_slack_ineq(int constraint_index, const std::vector<double> x_index,
		//					   const std::vector<double> x_factor, double min, double max);
		test_mpc.set_x_final_slack_ineq(0, {1}, {1.13}, -0.11, 0.12);
		test_mpc.set_x_final_slack_ineq_bound(0, -0.21, 0.22);
		//int set_delta_u_ae(unsigned int constraint_index, unsigned int control_index, double offset);
		test_mpc.set_delta_u_ae(0, 0, 0);
		//int set_delta_u_ae_offset(unsigned int constraint_index, double offset);
		test_mpc.set_delta_u_ae_offset(0, 0.1);
		//int set_delta_u_slack_ineq(unsigned int constraint_index, unsigned int u_index, double min, double max);
		test_mpc.set_delta_u_slack_ineq(0, 1, -1500, 1500);
		test_mpc.set_delta_u_slack_ineq_bound(0, -1500.1, 1500.1);
		//int set_delta_x_ae(unsigned int constraint_index, unsigned int x_index, double offset);
		test_mpc.set_delta_x_ae(0, 1, 0.0);
		test_mpc.set_delta_x_ae_offset(0, 0.3);
		//int set_delta_x_slack_ineq(unsigned int constraint_index, unsigned int x_index, double min, double max);
		test_mpc.set_delta_x_slack_ineq(0, 3, -1111, 1111);
		test_mpc.set_delta_x_slack_ineq_bound(0, -1111.1, 1111.1);
		/*	bool update_mpc(const Eigen::VectorXd &x0, const std::vector<double> &u_fb,
			const Eigen::MatrixXd &x_ref, const Eigen::MatrixXd &u_ref, const Eigen::MatrixXd &distur_ref,
			const Eigen::MatrixXd &q, const Eigen::MatrixXd &r, const std::vector<double> &xu_ae_w = {},
			const std::vector<double> &xu_slack_ineq_w = {}, const std::vector<double> &x_final_ae_w = {},
			const std::vector<double> &x_final_slack_ineq_w = {}, const std::vector<double> &delta_u_ae_w = {},
			const std::vector<double> &delta_u_slack_ineq_w = {}, const std::vector<double> &delta_x_ae_w = {},
			const std::vector<double> &delta_x_slack_ineq_w = {});*/
		test_mpc.update_mpc(x0, u_fb, x_ref, u_ref, distur_ref, q, r, {2.3}, {2.1}, {2.0}, {4.0}, {3.1}, {2.1}, {1.1}, {1.3});
		temp_matrix = Eigen::MatrixXd(test_mpc.hessian());
		std::cout << prefix + " hessian update1 is :" << std::endl;
		std::cout << temp_matrix << std::endl;
		eigen_to_csv(prefix + "_hessian_update1.csv", temp_matrix);
		//test defualt gradient
		std::cout << prefix + " gradient_update1 is:" << std::endl;
		std::cout << test_mpc.gradient().transpose() << std::endl;
		eigen_to_csv(prefix + "_gradient_update1.csv", test_mpc.gradient());
		temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
		std::cout << prefix + " constriant_matrix update1 is ok:" << std::endl;
		eigen_to_csv(prefix + "_constriant_matrix_update1.csv", test_mpc.constraint_matrix());
		std::cout << temp_matrix << std::endl;
		std::cout << prefix + " update1 bound is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_lowerBound_update1.csv", test_mpc.lowerBound());
		std::cout << test_mpc.upperBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_upperBound_update1.csv", test_mpc.upperBound());

		temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
		std::cout << prefix + " constriant_matrix affter update1  is:" << std::endl;
		std::cout << temp_matrix << std::endl;
		eigen_to_csv(prefix + "_constriant_matrix_update1.csv", temp_matrix);
		std::cout << prefix + " bound affter affter update1 is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_lowerBound_update1.csv", test_mpc.lowerBound());
		std::cout << test_mpc.upperBound().transpose() << std::endl;
		eigen_to_csv(prefix + "_upperBound_update1.csv", test_mpc.upperBound());
		test_mpc.mpc_solve();
	}
}